#include "State_Walking.h"
#include "Dog.h"

#define KP_F 4.0f
#define KD_F 0.1f

#define KP_B 4.0f
#define KD_B 0.1f

void Walking_Enter(void)
{
    UART_Print("Walking enter\n");

    /* 设置周期和站立占比 */
    Dog_SetPeriod(global.throttle, 0.5);

    global.isWalking = 1;

    global.bias[0] = 0.5;
    global.bias[1] = 0.0;
    global.bias[2] = 0.25;
    global.bias[3] = 0.75;

    /* 数据清零并开启定时器 */
    __HAL_TIM_SetCounter(&GLOBAL_TIM, 0);
    HAL_TIM_Base_Start(&GLOBAL_TIM);
}

#define TF 0.0f
#define TB 0.0f

void Walking_Run(void)
{
    // HT_SetPosition(dog.legs[0].motorF, -dog.legs[0].alpha - STARTANGLE_FF, KP_F, KD_F, TF);
    // HT_SetPosition(dog.legs[0].motorB, dog.legs[0].beta + STARTANGLE_FB, KP_B, KD_B, TB);
    // HT_SetPosition(dog.legs[1].motorF, -dog.legs[1].alpha + STARTANGLE_FF, KP_F, KD_F, TF);
    // HT_SetPosition(dog.legs[1].motorB, dog.legs[1].beta - STARTANGLE_FB, KP_B, KD_B, TB);
    // HT_SetPosition(dog.legs[2].motorF, -dog.legs[2].alpha - STARTANGLE_BF, KP_F, KD_F, TF);
    // HT_SetPosition(dog.legs[2].motorB, dog.legs[2].beta + STARTANGLE_BB, KP_B, KD_B, TB);
    // HT_SetPosition(dog.legs[3].motorF, -dog.legs[3].alpha + STARTANGLE_BF, KP_F, KD_F, TF);
    // HT_SetPosition(dog.legs[3].motorB, dog.legs[3].beta - STARTANGLE_BB, KP_B, KD_B, TB);
}

void Walking_Exit(void)
{
    UART_Print("Walking exit\n");

    global.isWalking = 0;
}
